/*
 * godos.c
 *
 *  Created on: 12.05.2011
 *      Author: Thomas Opolka
 *     Version: 0.87
 */

#include <stdlib.h>
#include <math.h>

#ifdef F_CPU
#include <avr/interrupt.h>
#endif

#ifndef _AVR_INTERRUPT_H_
#define cli()
#define sei()
#endif

#include "godos.h"


#define WHEEL_CIRCUMFERENCE 6.5 * M_PI  /* wheel circumference (centimeters) */
#define TIME_DIV 1000  /* milliseconds to seconds */


uint32_t dwGTimeOld = 0x00000000;  /* last time calling bGUpdateVelocity() */
uint32_t dwOTimeOld = 0x00000000;  /* last time calling bOUpdateVelocity() */


bool bGUpdateVelocity(volatile timeSinceStart_t *pdwTimeNow, fixedpoint_t *pwAcceleration, fixedpoint_t *pwGVelocity) {

	uint32_t dwGTimeDiff;
	fixedpoint_t wTimeDiff;

	cli(); /* disable global interrupts */
	/* calculate time difference and save current time value */
	dwGTimeDiff = *pdwTimeNow - dwGTimeOld;
	dwGTimeOld  = *pdwTimeNow;
	sei(); /* enable global interrupts */

	/* convert time difference */
	wTimeDiff = (uint16_t)((dwGTimeDiff << 8) / TIME_DIV);

	/* calculate momentary velocity (v = v0 + a*dt) */
	*pwGVelocity += fixedpoint_mul(*pwAcceleration, wTimeDiff);

	return true;
}


bool bOUpdateVelocity(volatile timeSinceStart_t *pdwTimeNow, fixedpoint_t *pwRevolutionLeft, fixedpoint_t *pwRevolutionRight, fixedpoint_t *pwOVelocity, fixedpoint_t *pwGVelocity) {

	uint32_t dwOTimeDiff;
	fixedpoint_t wRevolutionAvg;
	fixedpoint_t wVelocity;
	uint32_t dwDx;
	uint32_t dwDy;

	cli(); /* disable global interrupts */
	/* calculate time difference and save current time value */
	dwOTimeDiff = *pdwTimeNow - dwOTimeOld;
	dwOTimeOld  = *pdwTimeNow;
	sei(); /* enable global interrupts */

	/* determine average revolution number */
	wRevolutionAvg = (*pwRevolutionLeft + *pwRevolutionRight) >> 1; /* arithmetic average */

	/* calculate momentary velocity (v = dx/dt) */
	dwDx = (int32_t)fixedpoint_fromFloat(WHEEL_CIRCUMFERENCE) * (int32_t)wRevolutionAvg; /* dwDx >> 8 */
	dwDy = (dwOTimeDiff << 8) / (TIME_DIV / 100);
	wVelocity = (int16_t)(dwDx / dwDy);  /* dwDx << 8 */

//	/* apply sign of g_wGVelocity */
//	if (*pwGVelocity & (1 << 15)) {
//		wVelocity = -wVelocity;
//	}

	/* set g_wOVelocity */
	*pwOVelocity = wVelocity;

	/* reset revolution values */
	*pwRevolutionLeft = 0x0000;
	*pwRevolutionRight = 0x0000;

	return true;
}

